// Copyright 2025 Lihan Chen
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "pcd2pgm/pcd2pgm.hpp"

#include "pcl/common/transforms.h"
#include "pcl/filters/radius_outlier_removal.h"
#include "pcl/filters/voxel_grid.h"
#include "pcl/filters/statistical_outlier_removal.h"
#include "pcl/features/normal_3d.h"
#include "pcl/search/search.h"
#include "pcl/search/kdtree.h"
#include "pcl/io/pcd_io.h"
#include "pcl_conversions/pcl_conversions.h"
#include <thread>
#include <chrono>
#include <filesystem>

namespace pcd2pgm {
Pcd2PgmNode::Pcd2PgmNode(const rclcpp::NodeOptions &options)
    : Node("pcd2pgm", options), processing_completed_(false) {
    declareParameters();
    getParameters();

    std::cout << "=======================================" << std::endl;
    std::cout << "map_file_path: " << map_file_path_ << std::endl;
    std::cout << "map_file_name: " << map_file_name_ << std::endl;
    std::cout << "=======================================" << std::endl;

    rclcpp::QoS map_qos(10);
    map_qos.transient_local();
    map_qos.reliable();
    map_qos.keep_last(1);

    pcd_cloud_ = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
    map_publisher_ = this->create_publisher<nav_msgs::msg::OccupancyGrid>(
        map_topic_name_, map_qos);
    pcd_publisher_ =
        this->create_publisher<sensor_msgs::msg::PointCloud2>("pcd_cloud", 10);

    // 直接读取PCD文件并处理
    pcd_file_ = map_file_path_ + "/" + map_file_name_ + ".pcd";
    if (pcl::io::loadPCDFile<pcl::PointXYZ>(pcd_file_, *pcd_cloud_) == -1) {
        RCLCPP_ERROR(get_logger(), "Couldn't read file: %s",
                     pcd_file_.c_str());
        return;
    }

    applyTransform();

    // 创建保存目录
    if (enable_pcd_save_) {
        std::filesystem::create_directories(pcd_save_path_);
    }

    // 按步骤进行滤波处理
    auto current_cloud = pcd_cloud_;
    
    // 1. Z高度滤波
    passThroughFilter(current_cloud, thre_z_min_, thre_z_max_, flag_pass_through_);
    current_cloud = cloud_after_pass_through_;
    
    // 2. 体素滤波降低计算量
    voxelGridFilter(current_cloud, voxel_leaf_size_x_, voxel_leaf_size_y_, voxel_leaf_size_z_);
    current_cloud = cloud_after_voxel_;
    
    // 3. 统计离群点滤波剔除噪点
    statisticalOutlierFilter(current_cloud, statistical_mean_k_, statistical_std_dev_mul_);
    current_cloud = cloud_after_statistical_;
        
    // 4. 分块地面检测筛选地面点云并剔除（可选）
    if (enable_ground_removal_) {
        gridBasedGroundFilter(current_cloud, grid_size_x_, grid_size_y_,
                                min_points_per_grid_, ground_height_tolerance_,
                                ground_angle_threshold_);
        current_cloud = cloud_after_ground_removal_;
    } else {
        // 如果不启用地面移除，直接复制点云
        cloud_after_ground_removal_ = current_cloud;
        RCLCPP_INFO(get_logger(), "Step 4 - Ground removal disabled, keeping all points: %lu", 
                    current_cloud->points.size());
    }    // 最终的半径离群点滤波（可选，用于最后的精细过滤）
    radiusOutlierFilter(current_cloud, thre_radius_, thres_point_count_);
    
    setMapTopicMsg(cloud_after_radius_, map_topic_msg_);

    RCLCPP_INFO(get_logger(), "Initial point cloud size: %lu",
                pcd_cloud_->points.size());

    // 创建一个定时器来发布和保存地图，然后退出
    publish_timer_ = this->create_wall_timer(
        std::chrono::milliseconds(100),
        std::bind(&Pcd2PgmNode::publishCallback, this));

    // 创建退出检查定时器
    exit_timer_ = this->create_wall_timer(
        std::chrono::milliseconds(500),
        std::bind(&Pcd2PgmNode::checkForExit, this));
}

void Pcd2PgmNode::publishCallback() {
    if (processing_completed_) {
        return;
    }

    // 发布处理后的点云和地图
    sensor_msgs::msg::PointCloud2 output;
    pcl::toROSMsg(*cloud_after_radius_, output);
    output.header.frame_id = "map";
    pcd_publisher_->publish(output);
    map_publisher_->publish(map_topic_msg_);

    RCLCPP_INFO(get_logger(), "Published point cloud and occupancy grid map");

    // 增加延时1秒
    std::this_thread::sleep_for(std::chrono::seconds(1));

    // 保存地图文件
    std::string cmd = "ros2 run nav2_map_server map_saver_cli -f " +
                      map_file_path_ + "/" + map_file_name_;
    int ret = std::system(cmd.c_str());

    if (ret == -1) {
        RCLCPP_ERROR(this->get_logger(), "Failed to execute shell command");
    } else {
        RCLCPP_INFO(this->get_logger(), "Executed shell command: %s", cmd.c_str());
        RCLCPP_INFO(this->get_logger(), "Map saved successfully. Processing completed.");
    }

    // 标记处理完成
    processing_completed_ = true;
    
    // 停止发布定时器
    publish_timer_->cancel();
}

void Pcd2PgmNode::declareParameters() {
    declare_parameter("thre_z_min", 0.5);
    declare_parameter("thre_z_max", 2.0);
    declare_parameter("flag_pass_through", false);
    declare_parameter("thre_radius", 0.5);
    declare_parameter("map_resolution", 0.05);
    declare_parameter("thres_point_count", 10);
    declare_parameter("map_topic_name", "map");
    declare_parameter(
        "odom_to_lidar_odom",
        std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0, 0.0});

    // 体素滤波参数
    declare_parameter("voxel_leaf_size_x", 0.05);
    declare_parameter("voxel_leaf_size_y", 0.05);
    declare_parameter("voxel_leaf_size_z", 0.05);

    // 统计离群点滤波参数
    declare_parameter("statistical_mean_k", 30);
    declare_parameter("statistical_std_dev_mul", 1.0);

    // 地面移除控制参数
    declare_parameter("enable_ground_removal", true);

    // 分块地面检测参数
    declare_parameter("grid_size_x", 2.0);  // X方向网格大小（米）
    declare_parameter("grid_size_y", 2.0);  // Y方向网格大小（米）
    declare_parameter("min_points_per_grid", 50);  // 每个网格最少点数
    declare_parameter("ground_height_tolerance", 0.3);  // 地面高度容差（米）
    declare_parameter("ground_angle_threshold", 10.0);  // 地面法向量角度阈值（度）

    // PCD保存参数
    declare_parameter("enable_pcd_save", true);
    declare_parameter("pcd_save_path", "/home/yelume/01-program/03-浙商go2w/ros_ws/src/pcd2pgm/pcd/filtered_steps");

    // PCD文件路径参数
    declare_parameter("map_file_path", "/home/ubuntu/unitree_ws/00-PCD");
    declare_parameter("pcd_save.map_file_name", "0521_test");
}

void Pcd2PgmNode::getParameters() {
    get_parameter("thre_z_min", thre_z_min_);
    get_parameter("thre_z_max", thre_z_max_);
    get_parameter("flag_pass_through", flag_pass_through_);
    get_parameter("thre_radius", thre_radius_);
    get_parameter("map_resolution", map_resolution_);
    get_parameter("thres_point_count", thres_point_count_);
    get_parameter("map_topic_name", map_topic_name_);
    get_parameter("odom_to_lidar_odom", odom_to_lidar_odom_);

    // 获取新增滤波参数
    get_parameter("voxel_leaf_size_x", voxel_leaf_size_x_);
    get_parameter("voxel_leaf_size_y", voxel_leaf_size_y_);
    get_parameter("voxel_leaf_size_z", voxel_leaf_size_z_);
    
    get_parameter("statistical_mean_k", statistical_mean_k_);
    get_parameter("statistical_std_dev_mul", statistical_std_dev_mul_);
    
    get_parameter("enable_ground_removal", enable_ground_removal_);
    
    get_parameter("grid_size_x", grid_size_x_);
    get_parameter("grid_size_y", grid_size_y_);
    get_parameter("min_points_per_grid", min_points_per_grid_);
    get_parameter("ground_height_tolerance", ground_height_tolerance_);
    get_parameter("ground_angle_threshold", ground_angle_threshold_);
    
    get_parameter("enable_pcd_save", enable_pcd_save_);
    get_parameter("pcd_save_path", pcd_save_path_);

    // 获取PCD文件路径参数
    get_parameter("map_file_path", map_file_path_);
    get_parameter("pcd_save.map_file_name", map_file_name_);
}

void Pcd2PgmNode::passThroughFilter(const pcl::PointCloud<pcl::PointXYZ>::Ptr &input_cloud,
                                    double thre_low, double thre_high, bool flag_in) {
    auto filtered_cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
    pcl::PassThrough<pcl::PointXYZ> passthrough;
    passthrough.setInputCloud(input_cloud);
    passthrough.setFilterFieldName("z");
    passthrough.setFilterLimits(thre_low, thre_high);
    passthrough.setNegative(flag_in);
    passthrough.filter(*filtered_cloud);

    // 确保点云结构正确
    filtered_cloud->width = filtered_cloud->points.size();
    filtered_cloud->height = 1;
    filtered_cloud->is_dense = false;

    cloud_after_pass_through_ = filtered_cloud;
    RCLCPP_INFO(get_logger(), "Step 1 - After Z-height filtering: %lu points (removed %lu)",
                cloud_after_pass_through_->points.size(),
                input_cloud->points.size() - cloud_after_pass_through_->points.size());
    
    // 保存滤波后的点云
    if (enable_pcd_save_) {
        savePointCloudToPCD(cloud_after_pass_through_, "01_z_height_filtered.pcd", "Z高度滤波");
    }
}

void Pcd2PgmNode::radiusOutlierFilter(
    const pcl::PointCloud<pcl::PointXYZ>::Ptr &input_cloud, double radius,
    int thre_count) {
    auto filtered_cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
    pcl::RadiusOutlierRemoval<pcl::PointXYZ> radius_outlier;
    radius_outlier.setInputCloud(input_cloud);
    radius_outlier.setRadiusSearch(radius);
    radius_outlier.setMinNeighborsInRadius(thre_count);
    radius_outlier.filter(*filtered_cloud);

    // 确保点云结构正确
    filtered_cloud->width = filtered_cloud->points.size();
    filtered_cloud->height = 1;
    filtered_cloud->is_dense = false;

    cloud_after_radius_ = filtered_cloud;
    RCLCPP_INFO(get_logger(), "Step 5 - Final RadiusOutlier filtering: %lu points (removed %lu)",
                cloud_after_radius_->points.size(),
                input_cloud->points.size() - cloud_after_radius_->points.size());
    
    // 保存最终滤波后的点云
    if (enable_pcd_save_) {
        savePointCloudToPCD(cloud_after_radius_, "05_final_filtered.pcd", "最终滤波结果");
    }
}

void Pcd2PgmNode::setMapTopicMsg(
    const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
    nav_msgs::msg::OccupancyGrid &msg) {
    msg.header.stamp = now();
    msg.header.frame_id = "map";

    msg.info.map_load_time = now();
    msg.info.resolution = map_resolution_;

    double x_min = std::numeric_limits<double>::max();
    double x_max = std::numeric_limits<double>::lowest();
    double y_min = std::numeric_limits<double>::max();
    double y_max = std::numeric_limits<double>::lowest();

    if (cloud->points.empty()) {
        RCLCPP_WARN(get_logger(), "Point cloud is empty!");
        return;
    }

    for (const auto &point : cloud->points) {
        x_min = std::min(x_min, static_cast<double>(point.x));
        x_max = std::max(x_max, static_cast<double>(point.x));
        y_min = std::min(y_min, static_cast<double>(point.y));
        y_max = std::max(y_max, static_cast<double>(point.y));
    }

    msg.info.origin.position.x = x_min;
    msg.info.origin.position.y = y_min;
    msg.info.origin.position.z = 0.0;
    msg.info.origin.orientation.x = 0.0;
    msg.info.origin.orientation.y = 0.0;
    msg.info.origin.orientation.z = 0.0;
    msg.info.origin.orientation.w = 1.0;

    msg.info.width = std::ceil((x_max - x_min) / map_resolution_);
    msg.info.height = std::ceil((y_max - y_min) / map_resolution_);
    msg.data.assign(msg.info.width * msg.info.height, 0);

    for (const auto &point : cloud->points) {
        int i = std::floor((point.x - x_min) / map_resolution_);
        int j = std::floor((point.y - y_min) / map_resolution_);

        if (i >= 0 && i < msg.info.width && j >= 0 && j < msg.info.height) {
            msg.data[i + j * msg.info.width] = 100;
        }
    }

    RCLCPP_INFO(get_logger(), "Map data size: %lu", msg.data.size());
}

void Pcd2PgmNode::applyTransform() {
    Eigen::Affine3f transform = Eigen::Affine3f::Identity();

    transform.translation() << odom_to_lidar_odom_[0], odom_to_lidar_odom_[1],
        odom_to_lidar_odom_[2];
    transform.rotate(
        Eigen::AngleAxisf(odom_to_lidar_odom_[3], Eigen::Vector3f::UnitX()));
    transform.rotate(
        Eigen::AngleAxisf(odom_to_lidar_odom_[4], Eigen::Vector3f::UnitY()));
    transform.rotate(
        Eigen::AngleAxisf(odom_to_lidar_odom_[5], Eigen::Vector3f::UnitZ()));

    pcl::transformPointCloud(*pcd_cloud_, *pcd_cloud_, transform.inverse());
}

// 体素滤波函数
void Pcd2PgmNode::voxelGridFilter(const pcl::PointCloud<pcl::PointXYZ>::Ptr &input_cloud,
                                  double leaf_size_x, double leaf_size_y, double leaf_size_z) {
    auto filtered_cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
    pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;
    voxel_grid.setInputCloud(input_cloud);
    voxel_grid.setLeafSize(leaf_size_x, leaf_size_y, leaf_size_z);
    voxel_grid.filter(*filtered_cloud);

    // 确保点云结构正确
    filtered_cloud->width = filtered_cloud->points.size();
    filtered_cloud->height = 1;
    filtered_cloud->is_dense = false;

    cloud_after_voxel_ = filtered_cloud;
    RCLCPP_INFO(get_logger(), "Step 2 - After Voxel filtering: %lu points (reduced from %lu)",
                cloud_after_voxel_->points.size(), input_cloud->points.size());
    
    // 保存滤波后的点云
    if (enable_pcd_save_) {
        savePointCloudToPCD(cloud_after_voxel_, "02_voxel_filtered.pcd", "体素滤波");
    }
}

// 统计离群点滤波函数
void Pcd2PgmNode::statisticalOutlierFilter(const pcl::PointCloud<pcl::PointXYZ>::Ptr &input_cloud,
                                           int mean_k, double std_dev_mul) {
    auto filtered_cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> statistical_filter;
    statistical_filter.setInputCloud(input_cloud);
    statistical_filter.setMeanK(mean_k);
    statistical_filter.setStddevMulThresh(std_dev_mul);
    statistical_filter.filter(*filtered_cloud);

    // 确保点云结构正确
    filtered_cloud->width = filtered_cloud->points.size();
    filtered_cloud->height = 1;
    filtered_cloud->is_dense = false;

    cloud_after_statistical_ = filtered_cloud;
    RCLCPP_INFO(get_logger(), "Step 3 - After Statistical outlier filtering: %lu points (removed %lu outliers)",
                cloud_after_statistical_->points.size(), 
                input_cloud->points.size() - cloud_after_statistical_->points.size());
    
    // 保存滤波后的点云
    if (enable_pcd_save_) {
        savePointCloudToPCD(cloud_after_statistical_, "03_statistical_filtered.pcd", "统计离群点滤波");
    }
}

// PCD保存函数
void Pcd2PgmNode::savePointCloudToPCD(const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud,
                                      const std::string &filename, const std::string &step_name) {
    if (!cloud || cloud->empty()) {
        RCLCPP_WARN(get_logger(), "Cannot save empty point cloud for step: %s", step_name.c_str());
        return;
    }
    
    // 修复点云结构，确保width和height正确设置
    cloud->width = cloud->points.size();
    cloud->height = 1;
    cloud->is_dense = false;  // 设置为false以防有NaN点
    
    std::string full_path = pcd_save_path_ + "/" + filename;
    
    if (pcl::io::savePCDFileASCII(full_path, *cloud) == -1) {
        RCLCPP_ERROR(get_logger(), "Failed to save PCD file: %s", full_path.c_str());
    } else {
        RCLCPP_INFO(get_logger(), "Saved %s point cloud to: %s (%lu points)", 
                   step_name.c_str(), full_path.c_str(), cloud->points.size());
    }
}

// 分块地面检测函数
void Pcd2PgmNode::gridBasedGroundFilter(const pcl::PointCloud<pcl::PointXYZ>::Ptr &input_cloud,
                                        double grid_size_x, double grid_size_y,
                                        int min_points_per_grid, double height_tolerance,
                                        double angle_threshold) {
    if (input_cloud->empty()) {
        cloud_after_ground_removal_ = input_cloud;
        return;
    }

    RCLCPP_INFO(get_logger(), "Starting grid-based ground filtering with grid size: %.1f x %.1f", 
               grid_size_x, grid_size_y);

    // 计算点云边界
    float min_x = std::numeric_limits<float>::max();
    float max_x = std::numeric_limits<float>::lowest();
    float min_y = std::numeric_limits<float>::max();
    float max_y = std::numeric_limits<float>::lowest();

    for (const auto& point : input_cloud->points) {
        min_x = std::min(min_x, point.x);
        max_x = std::max(max_x, point.x);
        min_y = std::min(min_y, point.y);
        max_y = std::max(max_y, point.y);
    }

    // 计算网格数量
    int grid_cols = std::ceil((max_x - min_x) / grid_size_x);
    int grid_rows = std::ceil((max_y - min_y) / grid_size_y);
    
    RCLCPP_INFO(get_logger(), "Grid dimensions: %d x %d, covering area: %.1f x %.1f", 
               grid_cols, grid_rows, max_x - min_x, max_y - min_y);

    // 创建网格数据结构
    std::vector<std::vector<std::vector<int>>> grid(grid_rows, 
        std::vector<std::vector<int>>(grid_cols, std::vector<int>()));

    // 将点分配到网格中
    for (size_t i = 0; i < input_cloud->points.size(); ++i) {
        const auto& point = input_cloud->points[i];
        int grid_x = std::min(static_cast<int>((point.x - min_x) / grid_size_x), grid_cols - 1);
        int grid_y = std::min(static_cast<int>((point.y - min_y) / grid_size_y), grid_rows - 1);
        grid[grid_y][grid_x].push_back(i);
    }

    std::vector<bool> is_ground(input_cloud->points.size(), false);
    int processed_grids = 0;
    int total_ground_points = 0;

    // 对每个网格进行地面检测
    for (int row = 0; row < grid_rows; ++row) {
        for (int col = 0; col < grid_cols; ++col) {
            const auto& grid_points = grid[row][col];
            
            if (grid_points.size() < min_points_per_grid) {
                continue; // 点数太少，跳过此网格
            }

            processed_grids++;

            // 提取当前网格的点云
            pcl::PointCloud<pcl::PointXYZ>::Ptr grid_cloud(new pcl::PointCloud<pcl::PointXYZ>);
            std::vector<float> z_values;
            
            for (int idx : grid_points) {
                grid_cloud->points.push_back(input_cloud->points[idx]);
                z_values.push_back(input_cloud->points[idx].z);
            }

            // 计算当前网格的地面高度（使用最低的30%点的平均值）
            std::sort(z_values.begin(), z_values.end());
            size_t ground_sample_count = std::min(static_cast<size_t>(z_values.size() * 0.3), 
                                                 static_cast<size_t>(50));
            if (ground_sample_count < 5) ground_sample_count = std::min(z_values.size(), static_cast<size_t>(5));

            float local_ground_height = 0.0f;
            for (size_t i = 0; i < ground_sample_count; ++i) {
                local_ground_height += z_values[i];
            }
            local_ground_height /= ground_sample_count;

            // 计算法向量
            if (grid_cloud->points.size() >= 10) {
                pcl::search::Search<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
                pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
                pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
                
                grid_cloud->width = grid_cloud->points.size();
                grid_cloud->height = 1;
                grid_cloud->is_dense = false;
                
                normal_estimator.setSearchMethod(tree);
                normal_estimator.setInputCloud(grid_cloud);
                normal_estimator.setKSearch(10); // 使用K近邻而不是半径搜索
                normal_estimator.compute(*normals);

                // 标记地面点
                int grid_ground_points = 0;
                for (size_t i = 0; i < grid_points.size() && i < normals->points.size(); ++i) {
                    int original_idx = grid_points[i];
                    const auto& point = input_cloud->points[original_idx];
                    const auto& normal = normals->points[i];

                    // 检查高度条件
                    bool height_condition = std::abs(point.z - local_ground_height) <= height_tolerance;
                    
                    // 检查法向量条件
                    bool normal_condition = true;
                    if (!std::isnan(normal.normal_x) && !std::isnan(normal.normal_y) && !std::isnan(normal.normal_z)) {
                        Eigen::Vector3f point_normal(normal.normal_x, normal.normal_y, normal.normal_z);
                        point_normal.normalize();
                        Eigen::Vector3f vertical(0, 0, 1);
                        double angle_to_vertical = acos(std::abs(point_normal.dot(vertical))) * 180.0 / M_PI;
                        normal_condition = angle_to_vertical < angle_threshold;
                    }

                    if (height_condition && normal_condition) {
                        is_ground[original_idx] = true;
                        grid_ground_points++;
                        total_ground_points++;
                    }
                }
                
                if (processed_grids % 100 == 0) {
                    RCLCPP_INFO(get_logger(), "Processed %d grids, current grid [%d,%d]: %d/%lu points marked as ground, local height: %.3f", 
                               processed_grids, row, col, grid_ground_points, grid_points.size(), local_ground_height);
                }
            }
        }
    }

    // 构建过滤后的点云
    auto filtered_cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
    for (size_t i = 0; i < input_cloud->points.size(); ++i) {
        if (!is_ground[i]) {
            filtered_cloud->points.push_back(input_cloud->points[i]);
        }
    }

    // 确保点云结构正确
    filtered_cloud->width = filtered_cloud->points.size();
    filtered_cloud->height = 1;
    filtered_cloud->is_dense = false;

    cloud_after_ground_removal_ = filtered_cloud;
    
    RCLCPP_INFO(get_logger(), "Step 4 - Grid-based ground filtering completed:");
    RCLCPP_INFO(get_logger(), "  - Processed %d valid grids out of %d total grids", 
               processed_grids, grid_rows * grid_cols);
    RCLCPP_INFO(get_logger(), "  - Removed %d ground points, kept %lu points", 
               total_ground_points, filtered_cloud->points.size());
    
    // 保存滤波后的点云
    if (enable_pcd_save_) {
        savePointCloudToPCD(cloud_after_ground_removal_, "04_ground_removed.pcd", "分块地面移除");
    }
}

// 添加退出检查函数
void Pcd2PgmNode::checkForExit() {
    if (processing_completed_) {
        RCLCPP_INFO(get_logger(), "All processing completed. Shutting down node...");
        
        // 给一点时间让最后的日志输出
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
        
        // 关闭节点
        rclcpp::shutdown();
    }
}

} // namespace pcd2pgm

#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(pcd2pgm::Pcd2PgmNode)
